#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import argparse
import time
import os


class MoveJointWaypointPublisher(Node):
    def __init__(self, file_path: str):
        super().__init__('move_joint_waypoint_publisher')

        self.publisher_ = self.create_publisher(String, '/command_move_joint_waypoint', 10)

        if not os.path.exists(file_path):
            self.get_logger().warn(f"File {file_path} not found, but still publishing the path.")
        else:
            self.get_logger().info(f"Publishing waypoint file path: {file_path}")

        self.file_path = file_path

    def wait_for_subscribers(self, timeout=5.0):
        self.get_logger().info("Waiting for subscribers to connect...")
        start = time.time()
        while self.publisher_.get_subscription_count() == 0:
            if time.time() - start > timeout:
                self.get_logger().warn("Timeout waiting for subscribers!")
                return False
            rclpy.spin_once(self, timeout_sec=0.1)
            time.sleep(0.1)
        self.get_logger().info("Subscribers connected.")
        return True

    def send_file_path(self):
        msg = String()
        msg.data = self.file_path
        self.publisher_.publish(msg)
        self.get_logger().info(f"Published waypoint file path: {msg.data}")

        # 稍微等一下保证消息发出去
        rclpy.spin_once(self, timeout_sec=0.5)


def main():
    parser = argparse.ArgumentParser(description='Send move joint waypoint file path.')
    parser.add_argument('file_path', type=str, help='Path of the waypoint file.')
    args = parser.parse_args()

    rclpy.init()
    node = MoveJointWaypointPublisher(args.file_path)
    try:
        if node.wait_for_subscribers():
            node.send_file_path()
        else:
            node.get_logger().warn("No subscribers connected, aborting publish.")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
